#ifndef AGV_PKG__TASK_SERVER_HPP_
#define AGV_PKG__TASK_SERVER_HPP_

#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/string.hpp>
#include <memory>
#include <queue>
#include <vector>
#include "nav_msg_pkg/srv/send_task.hpp"
#include "nav_msg_pkg/srv/cancel_task.hpp"
#include "nav_msg_pkg/msg/waypoint.hpp"
#include "nav_msg_pkg/msg/nav_feedback.hpp"
#include "nav_msg_pkg/msg/task.hpp"

using Waypoint = nav_msg_pkg::msg::Waypoint;

/**
 * @brief 任务服务器类，用于接收任务请求并处理任务取消
 * 任务未完成时，不接收新任务
 */
namespace agv_pkg
{

    class TaskServer : public nav2_util::LifecycleNode
    {
    public:
        /**
         * @brief 构造函数
         * @param options 节点选项
         */
        explicit TaskServer(const rclcpp::NodeOptions &options = rclcpp::NodeOptions());

        /**
         * @brief 析构函数
         */
        ~TaskServer() override;

    protected:
        nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override;
        nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override;
        nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override;
        nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override;
        nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override;
        /**
         * @brief 处理任务请求的方法
         */
        void handle_task_request(const nav_msg_pkg::msg::Task &task,
                                 geometry_msgs::msg::PoseStamped &goal);

        /**
         * @brief 处理任务取消的方法
         */
        void handle_task_cancel();

        /**
         * @brief 任务请求回调函数
         * @param request 任务请求
         * @param response 任务响应
         */
        void task_request_callback(const std::shared_ptr<nav_msg_pkg::srv::SendTask::Request> request,
                                   std::shared_ptr<nav_msg_pkg::srv::SendTask::Response> response);

        /**
         * @brief 任务取消回调函数
         * @param request 任务取消请求
         * @param response 任务取消响应
         */
        void cancel_task_callback(const std::shared_ptr<nav_msg_pkg::srv::CancelTask::Request> request,
                                  std::shared_ptr<nav_msg_pkg::srv::CancelTask::Response> response);

        void navigation_feedback_callback(const nav_msg_pkg::msg::NavFeedback::SharedPtr feedback);
        /**
         * 导航结果回调函数
         */
        void navigation_result_callback(const std_msgs::msg::String::SharedPtr &result);

        void task_timer_callback();
        // 使用智能指针管理资源
        std::shared_ptr<rclcpp::Service<nav_msg_pkg::srv::SendTask>> task_request_service_;
        std::shared_ptr<rclcpp::Service<nav_msg_pkg::srv::CancelTask>> task_cancel_service_;

        // 导航位姿发布
        rclcpp::Publisher<nav_msg_pkg::msg::Waypoint>::SharedPtr navigation_pose_pub_;
        // 导航进度订阅
        rclcpp::Subscription<nav_msg_pkg::msg::NavFeedback>::SharedPtr navigation_feedback_sub_;
        // 导航结果订阅
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr navigation_result_sub_;
        // 导航位姿
        std::vector<nav_msg_pkg::msg::Waypoint> points_;

        // 任务管理
        // 任务是否正在运行
        bool is_task_running_ = false;
        std::queue<nav_msg_pkg::msg::Task> task_queue_;
        // std::list<nav_msg_pkg::msg::Task> task_list_;
        // bool task_exists(const std::string &task_id){
        //     for(const auto &task : task_list_){
        //         if(task.task_id == task_id){
        //             return true;
        //         }
        //     }
        //     return false;
        // }
        rclcpp::TimerBase::SharedPtr task_timer_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr navigation_cancel_pub_;
    };
}

#endif // AGV_PKG__TASK_SERVER_HPP_